#include <SD.h>
#include <Wire.h>
#include <Servo.h>
#include "RTClib.h"

#define trigPin 8
#define echoPin 9

RTC_PCF8523 rtc;

#define LOG_INTERVAL 1000
#define SYNC_INTERVAL 1000

uint32_t syncTime = 0;

File logfile;
long duration;
int distance;
Servo myservo;

int calculateDistance()
{
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distance = duration * 0.034 / 2;
    return distance;
}

void error(char const *str)
{
    Serial.print("error: ");
    Serial.println(str);

    while (1)
        ;
}
void createLogFile()
{
    // 创建新文件
    char filename[] = "MLOG00.CSV";
    for (uint8_t i = 0; i < 100; i++)
    {
        filename[4] = i / 10 + '0';
        filename[5] = i % 10 + '0';
        if (!SD.exists(filename))
        {
            logfile = SD.open(filename, FILE_WRITE);
            break;
        }
    }

    if (!logfile)
    {
        error("无法创建文件");
    }

    // Serial.print("Logging to: ");
    // Serial.println(filename);
}
void setup()
{
    Serial.begin(9600);

    // 初始化SD卡
    //  Serial.println("Initializing SD card...");
    pinMode(10, OUTPUT);
    if (!SD.begin(10))
    {
        Serial.println("SD 卡初始化失败");
        return;
    }

    // Serial.println("card initialized.");

    createLogFile();

    // 初始化RTC模块
    Wire.begin();
    if (!rtc.begin())
    {
        logfile.println("RTC failed");
        Serial.println("Couldn't find RTC");
        // while (1);
    }

    // 初始化超声波传感器
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);
    myservo.attach(7);
    // 初始化舵机
    // 你的舵机初始化代码

    // 输出标题行
    logfile.println("millis,stamp,datetime,degree,distance");
    // Serial.println("millis,stamp,datetime,degree,distance");
}

// void logData(int degree)
// {
//     DateTime now = rtc.now();
//     logfile.print(millis());
//     logfile.print(",");
//     logfile.print(now.unixtime());
//     logfile.print(",");
//     logfile.print(now.year(), DEC);
//     logfile.print("/");
//     logfile.print(now.month(), DEC);
//     logfile.print("/");
//     logfile.print(now.day(), DEC);
//     logfile.print(" ");
//     logfile.print(now.hour(), DEC);
//     logfile.print(":");
//     logfile.print(now.minute(), DEC);
//     logfile.print(":");
//     logfile.print(now.second(), DEC);
//     logfile.print(",");
//     logfile.print(degree);
//     logfile.print(",");
//     logfile.println(distance);
//     // if ((millis() - syncTime) >= SYNC_INTERVAL)
//     {
//     // syncTime = millis();
//     Serial.println(millis());
//     logfile.flush();
//     }
// }
int m = 0;

unsigned long previousMillis = 0;
const long interval = 1000; // 1000毫秒（1秒）记录一次

void loop()
{

    int i;
    unsigned long currentMillis = millis();
    int startDegree = 0;
    int endDegree = 180;
    int u = 1; // 控制角度变化的步进
    // 从 15 度到 165 度递增
    for (i = startDegree; i <= endDegree + 1; i = i + u)
    {
        if (i >= endDegree + 1)
        {
            i = i - 2;
            u = -u;
        }
        if (i <= startDegree - 1)
        {
            // break;
            i+=2;
            u=-u;
        }
        myservo.write(i);
        delay(15);
        
        calculateDistance();
        // Serial.println(String(i) + ","+String(u));
        // Serial.print(i);
        // Serial.print(",");
        // Serial.print(distance);
        // Serial.print(".");
        // 检查是否到了记录时间
        // if (currentMillis - previousMillis >= 999)
        // {
        //     previousMillis = currentMillis; // 更新上次记录的时间
        //     logData(i);
        // }
        if ((999 - millis() % 1000) >= 960)
        {
            // Serial.println(millis());
            // logData(i);
            DateTime now = rtc.now();
            logfile.print(millis());
            logfile.print(",");
            logfile.print(now.unixtime());
            logfile.print(",");
            logfile.print(now.year(), DEC);
            logfile.print("/");
            logfile.print(now.month(), DEC);
            logfile.print("/");
            logfile.print(now.day(), DEC);
            logfile.print(" ");
            logfile.print(now.hour(), DEC);
            logfile.print(":");
            logfile.print(now.minute(), DEC);
            logfile.print(":");
            logfile.print(now.second(), DEC);
            logfile.print(",");
            logfile.print(i);
            logfile.print(",");
            logfile.println(distance);
            // if ((millis() - syncTime) >= SYNC_INTERVAL)
            {
                // syncTime = millis();
                Serial.println(millis());
                logfile.flush();
            }
        }
    }
}

// int startDegree = 0;
// int endDegree = 180;
// void loop()
// {
//     int i;
//     int u = 1;

//     // 从 15 度到 165 度递增
//     for (i = startDegree; i <= endDegree + 1; i = i + u)
//     {

//         if (i >= endDegree + 1)
//             u = -u;
//         if (i <= startDegree - 1)
//             break;
//         myservo.write(i);
//         delay(15);
//         m++;
//         calculateDistance();
//         Serial.print(i);
//         Serial.print(",");
//         Serial.print(distance);
//         Serial.print(".");
//         if (m > 54)
//         {
//             m = 0;
//             logData(i);
//         }
//     }

// // 从 165 度到 15 度递减
// for (i = 165; i >= 15; i--)
// {
//     myservo.write(i);
//     delay(15);
//     if(m>50)
//     {
//     calculateDistance();
//     Serial.print(i);
//     Serial.print(",");
//     Serial.print(distance);
//     Serial.print(".");

//     m=0;
//     logData(i);
//     }
// }
// }
